#ifndef __FIRST_PERSON_CAMERA_H__
#define __FIRST_PERSON_CAMERA_H__

#include "camera.h"
#include "model/matrix4x4.h"
#include <iostream>

class FirstPersonCamera : public Camera {
	private:
		float theta_lr;
		float theta_ud;
	public:
		FirstPersonCamera(Point at = Point(0,0,0),Point eye = Point(3.0f, 3.0f, 3.0f),Point up=Point(0,0,1))
					: Camera(	eye,
								at,
								up){
			float m = sqrt(pow(eye.x1-at.x1,2)+pow(eye.x2-at.x2,2));
			theta_ud = atan2(eye.x3-at.x3,m);
			theta_lr = 0.0f;
		}

		~FirstPersonCamera(){

		}


		void applyCamera(){
			glMatrixMode(GL_MODELVIEW);
			glLoadIdentity();

			gluLookAt(eye.x1, eye.x2, eye.x3, at.x1, at.x2, at.x3, up.x1, up.x2, up.x3);

		}

		void rotateUD(float theta){
			// Grados a Radianes
			theta = -theta*PI/180;

			float yzang = atan2(eye.x1-at.x1,eye.x2-at.x2);

			Matrix4x4 t1(-eye.x1,-eye.x2,-eye.x3);
			Matrix4x4 r1(yzang,Point(0,0,1));
			Matrix4x4 r2(theta,Point(1,0,0));
			Matrix4x4 r3(-yzang,Point(0,0,1));
			Matrix4x4 t2(eye.x1,eye.x2,eye.x3);

			Matrix4x4 t;
			t *= t1;
			t *= r1;
			t *= r2;
			t *= r3;
			t *= t2;

			this->eye=t*this->eye;
			this->at=t*this->at;
		}

		void move(float offset){
			float m = sqrt(pow(eye.x1-at.x1,2)+pow(eye.x2-at.x2,2));
			float xyang = atan2(eye.x3-at.x3,m);
			float yzang = atan2(eye.x1-at.x1,eye.x2-at.x2);

			Matrix4x4 t1(-eye.x1,-eye.x2,-eye.x3);
			Matrix4x4 r1(yzang,Point(0,0,1));
			Matrix4x4 r2(-xyang,Point(1,0,0));
			Matrix4x4 t2(0,offset,0);
			Matrix4x4 r3(xyang,Point(1,0,0));
			Matrix4x4 r4(-yzang,Point(0,0,1));
			Matrix4x4 t3(eye.x1,eye.x2,eye.x3);

			Matrix4x4 t;
			t *= t1;
			t *= r1;
			t *= r2;
			t *= t2;
			t *= r3;
			t *= r4;
			t *= t3;

			this->eye=t*this->eye;
			this->at=t*this->at;
		}

		void rotateLR(float theta){
			// Grados a Radianes
			theta = -theta*PI/180;

			Matrix4x4 t1(-eye.x1,-eye.x2,-eye.x3);
			Matrix4x4 r1(theta,Point(0,0,1));
			Matrix4x4 t2(eye.x1,eye.x2,eye.x3);

			Matrix4x4 t;
			t *= t1;
			t *= r1;
			t *= t2;

			this->eye=t*this->eye;
			this->at=t*this->at;
		}

		void updatePosition(const PathIterator& it){
			this->eye=it.getCurrent();
			this->at=it.getLast();

			this->eye.x3+=0.5f;
			this->at.x3+=0.5f;
		}
};

#endif
